/* @file event_manager.h
 * @brief This file contains the declaration of the event manager class.
 *
 * This file provides the declaration of the EventManager class, which is used to
 * manage events and report them. The class definition includes a constructor and
 * some member functions.
 *
 * @author haibo.yang
 * @date 2023-06-09
 */

#pragma once

#include "state_monitor/monitor.h"
#include "state_monitor/micros.h"
#include <realtime_tools/realtime_buffer.hpp>
#include <realtime_tools/realtime_publisher.hpp>

#include <agv_msgs/msg/agv_event_status.hpp>
#include <agv_msgs/msg/agv_event.hpp>

#include <boost/thread/thread.hpp> 
#define AGVEVENTTIMEROUT 1.5

namespace monitor {

#define AGVEvent_TO_STREAM(stream, event) \
    stream << "AGVEvent:\n"; \
    stream << "  header:\n"; \
    stream << "    stamp: " << event.header.stamp.sec << "." << std::setfill('0') << std::setw(9) << event.header.stamp.nanosec << "\n"; \
    stream << "    frame_id: " << event.header.frame_id << "\n"; \
    stream << "  type: " << static_cast<int>(event.type) << "\n"; \
    stream << "  action: " << static_cast<int>(event.action) << "\n"; \
    stream << "  ackreq: " << static_cast<int>(event.ackreq) << "\n"; \
    stream << "  eventtime: " << std::fixed << std::setprecision(3) << event.eventtime << "s\n"; \
    stream << "  vellimit: " << event.vellimit << "m/s\n"; \
    stream << "  ecode: " << event.ecode << "\n"; \
    stream << "  param1: " << event.param1 << "\n"; \
    stream << "  param2: " << event.param2 << "\n"; \
    stream << "  param3: " << event.param3 << "\n"; \
    stream << "  param4: " << event.param4 << "\n"; \
    stream << "  description_cn: \"" << event.description_cn << "\"\n"; \
    stream << "  description: \"" << event.description << "\""

inline std::ostream& operator<<(std::ostream& os, const agv_msgs::msg::AGVEvent& event) {
  AGVEvent_TO_STREAM(os, event);
  return os;
}

inline std::string gid_to_hex_string(const rmw_gid_t& gid) {
  std::stringstream ss;
  ss << std::hex << std::setfill('0');
  for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; ++i) {
    ss << std::setw(2) << static_cast<unsigned int>(gid.data[i]);
  }
  return ss.str();
}

 
template <size_t N>
inline std::string gid_to_hex_string(const std::array<unsigned char, N>& arr) {
  std::stringstream ss;
  ss << std::hex << std::setfill('0');
  for (size_t i = 0; i < N; ++i) {
    ss << std::setw(2) << static_cast<unsigned int>(arr[i]);
  }
  return ss.str();
}

// Centralized event manager.
class EventManager {
public:
  virtual ~EventManager();

  void ShutDown();

  /// @brief Class initialization function 
  void Init(const double interval = 0.01f);

  /// Getters.
  AgvEventMap* GetEventMap() { return &event_map_; }
  
  /// @brief escalate events to event manager
  /// @param name node name
  /// @param event node event
  static void EscalateEvents(const std::string& name, const agv_msgs::msg::AGVEvent::SharedPtr& event, const std::string& gid = "");
private:
  /// 
  void run();

  /// @brief event callback 
  /// @param event node event
  void EventCallBack(const agv_msgs::msg::AGVEvent::SharedPtr event, const rclcpp::MessageInfo& info);
private:
  /// @brief node name
  std::string name_;

  /// @brief interval
  double interval_;

  /// @brief event map
  AgvEventMap event_map_;

  // /// @brief Input statuses.
  // std::string current_mode_;

  /// @brief current event
  std::pair<std::string, agv_msgs::msg::AGVEvent> current_event_;

  /// @brief timeout events
  std::vector<std::pair<std::string, agv_msgs::msg::AGVEvent>> timeout_events_;

  /// @brief agv event subscriber
  rclcpp::Subscription<agv_msgs::msg::AGVEvent>::SharedPtr agv_event_sub_;

  /// @brief publish event action
  rclcpp::Publisher<agv_msgs::msg::AGVEventStatus>::SharedPtr status_pub_;

  /// @brief event publish thread; 
  boost::thread event_thread_;

  // event manager instance
  DECLARE_SINGLETON(EventManager)
};

}  // namespace monitor